#include "MeasurE616AlgBackL1ToL8Circle.h"

MeasurE616AlgBackL1toL8Circle::MeasurE616AlgBackL1toL8Circle(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgBackL1toL8Circle::operator()(const cv::Mat &srcImgTemp,
                                               int id,
                                               std::map<string, float> &measRes,
                                               int fixtureId)
{
    qDebug() << "E616 alg back l1 to l8 circle start";

    if(!srcImgTemp.data) {
        qDebug() << "E616 input empty image";
        return;
    }

    cv::Mat srcImg = srcImgTemp.clone();
    MeasurE616EdgeDetect obj;
    //背光图小半圆边缘提取  // CDL back图    \paras616\back\ cdL.xml
//    int NewCircleEdge(cv::Mat srcimg, const vector<int> paras, vector<cv::Point>& contours);
    std::vector<cv::Point> resptVec;

    for(auto i=0;i<8;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        obj.NewCircleEdge(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec.push_back(ptsOut);
        }
    }
    std::vector<float> cirres;
    cirres=obj.FitCircle(resptVec);

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI76-L-X-distance_pixel"), -cirres[0]));
    float fai76dx = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI76-L-X-distance")], -cirres[0]);
    measRes.insert(std::make_pair(string("FAI76-L-X-distance_real"), fai76dx));

    measRes.insert(std::make_pair(string("FAI76-L-Y-distance_pixel"), -cirres[1]));
    float fai76dy = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI76-L-Y-distance")], -cirres[1]);
    measRes.insert(std::make_pair(string("FAI76-L-Y-distance_real"), fai76dy));

    measRes.insert(std::make_pair(string("FAI75-L-diam_pixel"), cirres[2]*2));
    float fai75dd = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI75-L-diam")], cirres[2]*2);
    measRes.insert(std::make_pair(string("FAI75-L-diam_real"), fai75dd));

//    TODO
//    float fai76pos=obj.GetPosition(cv::Point2f(-5.969,-5.804),cv::Point2f(fai76dx,fai76dy));
//    measRes.insert(std::make_pair(string("FAI76-L-position"), fai76pos));

//    cv::Point2f stdposFai76(-5.969, -5.804);

    float fai76PosPixelX = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI76-L-X-distance")],
            m_standardValMap["FAI76-L-X-distance"]);
    float fai76PosPixelY = CalcPixelAccordExpectDist(m_phypixpara[prefix + std::string("FAI76-L-Y-distance")],
            m_standardValMap["FAI76-L-Y-distance"]);

    double fai76PosPixel = obj.GetPosition(cv::Point2f(-cirres[0], -cirres[1]), cv::Point2f(fai76PosPixelX, fai76PosPixelY));
    measRes.insert(std::make_pair(string("FAI76-L-position_pixel"), fai76PosPixel));
    float fai76PosDis = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI76-L-position")], fai76PosPixel);
    measRes.insert(std::make_pair(string("FAI76-L-position_real"), fai76PosDis));

    qDebug() << "E616 alg back l1 to l8 circle end";
}
